use std::{sync::Arc, time::Duration};

use futures::{select, FutureExt, TryFutureExt};
use tokio::{
    runtime::{self, Runtime},
    sync::Notify,
    time::timeout,
};

use crate::*;

fn connect(uri: &str, timeout_ms: u64) -> Result<Box<ClientWraper>> {
    let rt = runtime::Builder::new_current_thread()
        .enable_all()
        .build()
        .map_err(|e| Error::from(e))?;

    {
        let _guard = rt.handle().enter();

        let fut = timeout(Duration::from_millis(timeout_ms), Client::connect(uri))
            .map_err(|e| Error::from(e));
        rt.block_on(fut).and_then(|inner| {
            inner.map(|inner| {
                Box::new(ClientWraper {
                    rt: Arc::new(rt),
                    inner,
                })
            })
        })
    }
}

fn recycle(
    vch: Box<VehicleControlHandleWrapper>,
    sh: Box<SensorHandleWrapper>,
) -> Box<ClientWraper> {
    Box::new(ClientWraper {
        rt: (*vch).rt,
        inner: Client::recycle((*vch).inner, (*sh).inner),
    })
}

fn create_vehicle(
    client: Box<ClientWraper>,
    client_id: &str,
    vehicle_type: &str,
    position_xyz: [f64; 3],
    orientation_wxyz: [f64; 4],
    timeout_ms: u64,
) -> Result<Box<HandleWrapper>> {
    let rt = client.rt;
    {
        let _guard = rt.handle().enter();

        let fut = timeout(
            Duration::from_millis(timeout_ms),
            client.inner.create_vehicle(
                client_id,
                vehicle_type,
                (position_xyz[0], position_xyz[1], position_xyz[2]),
                (
                    orientation_wxyz[0],
                    orientation_wxyz[1],
                    orientation_wxyz[2],
                    orientation_wxyz[3],
                ),
            ),
        )
        .map_err(|e| Error::from(e));
        rt.block_on(fut).and_then(|r| {
            r.map(|(vch, sh)| {
                let (sh, ssh) = SensorHandleWrapper::new(rt.clone(), sh);

                Box::new(HandleWrapper {
                    vch: Some(VehicleControlHandleWrapper { rt, inner: vch }),
                    sh: Some(sh),
                    ssh: Some(ssh),
                })
            })
        })
    }
}

fn get_vehicle(
    client: Box<ClientWraper>,
    client_id: &str,
    token: &str,
    timeout_ms: u64,
) -> Result<Box<HandleWrapper>> {
    let rt = client.rt;
    {
        let _guard = rt.handle().enter();

        let fut = timeout(
            Duration::from_millis(timeout_ms),
            client.inner.get_vehicle(client_id, token),
        )
        .map_err(|e| Error::from(e));
        rt.block_on(fut).and_then(|r| {
            r.map(|(vch, sh)| {
                let (sh, ssh) = SensorHandleWrapper::new(rt.clone(), sh);

                Box::new(HandleWrapper {
                    vch: Some(VehicleControlHandleWrapper { rt, inner: vch }),
                    sh: Some(sh),
                    ssh: Some(ssh),
                })
            })
        })
    }
}

struct ClientWraper {
    rt: Arc<Runtime>,
    inner: Client,
}

struct HandleWrapper {
    vch: Option<VehicleControlHandleWrapper>,
    sh: Option<SensorHandleWrapper>,
    ssh: Option<StopSensorHandleWrapper>,
}

impl HandleWrapper {
    fn get_vehicle_control_handle(&mut self) -> Result<Box<VehicleControlHandleWrapper>> {
        if let Some(vch) = self.vch.take() {
            Ok(Box::new(vch))
        } else {
            Err(Error::new("None".to_string()))
        }
    }

    fn get_sensor_handle(&mut self) -> Result<Box<SensorHandleWrapper>> {
        if let Some(sh) = self.sh.take() {
            Ok(Box::new(sh))
        } else {
            Err(Error::new("None".to_string()))
        }
    }

    fn get_stop_sensor_handle(&mut self) -> Result<Box<StopSensorHandleWrapper>> {
        if let Some(ssh) = self.ssh.take() {
            Ok(Box::new(ssh))
        } else {
            Err(Error::new("None".to_string()))
        }
    }
}

struct VehicleControlHandleWrapper {
    rt: Arc<Runtime>,
    inner: VehicleControlHandle,
}

impl VehicleControlHandleWrapper {
    fn set_control(&mut self, control: &ffi::VehicleControl, timeout_ms: u64) -> Result<()> {
        let _guard = self.rt.handle().enter();

        let control = VehicleControl {
            throttle: control.throttle,
            steer: control.steer,
            brake: control.brake,
            hand_brake: control.hand_brake,
            reverse: control.reverse,
            manual_gear_shift: control.manual_gear_shift,
            gear: control.gear,
        };
        let fut = timeout(
            Duration::from_millis(timeout_ms),
            self.inner.set_control(&control),
        )
        .map_err(|e| Error::from(e));
        self.rt.block_on(fut).and_then(|r| r)
    }

    fn set_transform(
        &mut self,
        position_xyz: &[f64; 3],
        orientation_wxyz: &[f64; 4],
    ) -> Result<()> {
        let _guard = self.rt.handle().enter();

        self.rt.block_on(self.inner.set_transform(
            (position_xyz[0], position_xyz[1], position_xyz[2]),
            (
                orientation_wxyz[0],
                orientation_wxyz[1],
                orientation_wxyz[2],
                orientation_wxyz[3],
            ),
        ))
    }
}

struct SensorHandleWrapper {
    rt: Arc<Runtime>,
    inner: SensorHandle,
    notify: Arc<Notify>,
}

impl SensorHandleWrapper {
    fn new(rt: Arc<Runtime>, inner: SensorHandle) -> (Self, StopSensorHandleWrapper) {
        let notify = Arc::new(Notify::new());

        (
            Self {
                rt,
                inner,
                notify: notify.clone(),
            },
            StopSensorHandleWrapper { notify },
        )
    }

    fn get_sensors(&mut self) -> Result<ffi::Sensors> {
        let _guard = self.rt.handle().enter();

        let fut = async {
            let notify = self.notify.clone();

            select! {
                sensors = self.inner.get_sensors().fuse() => {
                    sensors
                }

                _ = notify.notified().fuse() => {
                    Err(Error::new("Stop".into()))
                }
            }
        };

        self.rt.block_on(fut).map(|sensors| {
            let test_result = if let Some(test_result) = sensors.test_result {
                let success = test_result.success;
                let mut successful_criteria = Vec::new();
                let mut failed_criteria = Vec::new();
                test_result.criteria.into_iter().for_each(|(k, v)| {
                    if v {
                        successful_criteria.push(k);
                    } else {
                        failed_criteria.push(k);
                    }
                });

                vec![ffi::TestResult {
                    success,
                    successful_criteria,
                    failed_criteria,
                }]
            } else {
                Vec::new()
            };

            let localization = if let Some(localization) = sensors.localization {
                vec![ffi::LocalizationInfo {
                    timestamp: localization.timestamp,
                    cartesian_lla: [
                        localization.cartesian_lla.0,
                        localization.cartesian_lla.1,
                        localization.cartesian_lla.2,
                    ],
                    position_xyz: [
                        localization.position_xyz.0,
                        localization.position_xyz.1,
                        localization.position_xyz.2,
                    ],
                    orientation_wxyz: [
                        localization.orientation_wxyz.0,
                        localization.orientation_wxyz.1,
                        localization.orientation_wxyz.2,
                        localization.orientation_wxyz.3,
                    ],
                    linear_vel_xyz: [
                        localization.linear_vel_xyz.0,
                        localization.linear_vel_xyz.1,
                        localization.linear_vel_xyz.2,
                    ],
                    linear_accel_xyz: [
                        localization.linear_accel_xyz.0,
                        localization.linear_accel_xyz.1,
                        localization.linear_accel_xyz.2,
                    ],
                    angular_vel_xyz: [
                        localization.angular_vel_xyz.0,
                        localization.angular_vel_xyz.1,
                        localization.angular_vel_xyz.2,
                    ],
                }]
            } else {
                Vec::new()
            };

            let perception = if let Some(perception) = sensors.perception {
                let obstacles = perception
                    .obstacles
                    .into_iter()
                    .map(|obstacle| ffi::ObstacleInfo {
                        id: obstacle.id,
                        o_type: {
                            match obstacle.o_type {
                                ObstacleType::Unknow => ffi::ObstacleType::Unknow,
                                ObstacleType::Pedestrian => ffi::ObstacleType::Pedestrian,
                                ObstacleType::Vehicle => ffi::ObstacleType::Vehicle,
                            }
                        },
                        is_static: obstacle.is_static,
                        bounding_box: ffi::BoundingBox {
                            position_xyz: [
                                obstacle.bounding_box.position_xyz.0,
                                obstacle.bounding_box.position_xyz.1,
                                obstacle.bounding_box.position_xyz.2,
                            ],
                            orientation_wxyz: [
                                obstacle.bounding_box.orientation_wxyz.0,
                                obstacle.bounding_box.orientation_wxyz.1,
                                obstacle.bounding_box.orientation_wxyz.2,
                                obstacle.bounding_box.orientation_wxyz.3,
                            ],
                            extent_xyz: [
                                obstacle.bounding_box.extent_xyz.0,
                                obstacle.bounding_box.extent_xyz.1,
                                obstacle.bounding_box.extent_xyz.2,
                            ],
                        },
                        position_xyz: [
                            obstacle.position_xyz.0,
                            obstacle.position_xyz.1,
                            obstacle.position_xyz.2,
                        ],
                        orientation_wxyz: [
                            obstacle.orientation_wxyz.0,
                            obstacle.orientation_wxyz.1,
                            obstacle.orientation_wxyz.2,
                            obstacle.orientation_wxyz.3,
                        ],
                        linear_vel_xyz: [
                            obstacle.linear_vel_xyz.0,
                            obstacle.linear_vel_xyz.1,
                            obstacle.linear_vel_xyz.2,
                        ],
                    })
                    .collect();

                let traffic_light = if let Some(tl) = perception.traffic_light {
                    vec![ffi::TrafficLightInfo {
                        timestamp: tl.timestamp,
                        position_xyz: [tl.position_xyz.0, tl.position_xyz.1, tl.position_xyz.2],
                        orientation_wxyz: [
                            tl.orientation_wxyz.0,
                            tl.orientation_wxyz.1,
                            tl.orientation_wxyz.2,
                            tl.orientation_wxyz.3,
                        ],
                        state: match tl.state {
                            TrafficLightState::Red => ffi::TrafficLightState::Red,
                            TrafficLightState::Yellow => ffi::TrafficLightState::Yellow,
                            TrafficLightState::Green => ffi::TrafficLightState::Green,
                            TrafficLightState::Off => ffi::TrafficLightState::Off,
                            TrafficLightState::Unknown => ffi::TrafficLightState::Unknown,
                        },
                    }]
                } else {
                    Vec::new()
                };

                vec![ffi::PerceptionInfo {
                    timestamp: perception.timestamp,
                    obstacles,
                    traffic_light,
                }]
            } else {
                Vec::new()
            };

            let chassis = if let Some(chassis) = sensors.chassis {
                vec![ffi::ChassisInfo {
                    timestamp: chassis.timestamp,
                    control: ffi::VehicleControl {
                        throttle: chassis.control.throttle,
                        steer: chassis.control.steer,
                        brake: chassis.control.brake,
                        hand_brake: chassis.control.hand_brake,
                        reverse: chassis.control.reverse,
                        manual_gear_shift: chassis.control.manual_gear_shift,
                        gear: chassis.control.gear,
                    },
                }]
            } else {
                Vec::new()
            };

            let collision_event = if let Some(ce) = sensors.collision_event {
                vec![ffi::CollisionEvent {
                    timestamp: ce.timestamp,
                    normal_impulse: [
                        ce.normal_impulse.0,
                        ce.normal_impulse.1,
                        ce.normal_impulse.2,
                    ],
                }]
            } else {
                Vec::new()
            };

            let lane_invasion_event = if let Some(lie) = sensors.lane_invasion_event {
                vec![ffi::LaneInvasionEvent {
                    timestamp: lie.timestamp,
                    lane_change: lie
                        .lane_change
                        .into_iter()
                        .map(|c| match c {
                            LaneChangeType::None => ffi::LaneChangeType::None,
                            LaneChangeType::Right => ffi::LaneChangeType::Right,
                            LaneChangeType::Left => ffi::LaneChangeType::Left,
                            LaneChangeType::Both => ffi::LaneChangeType::Both,
                        })
                        .collect(),
                }]
            } else {
                Vec::new()
            };

            let camera_images = sensors
                .camera_images
                .into_iter()
                .map(|i| ffi::Image {
                    timestamp: i.timestamp,
                    camera_id: i.camera_id,
                    width: i.width,
                    height: i.height,
                    is_gray: i.is_gray,
                    data: i.data,
                })
                .collect();

            ffi::Sensors {
                test_result,
                localization,
                perception,
                chassis,
                collision_event,
                lane_invasion_event,
                camera_images,
            }
        })
    }
}

struct StopSensorHandleWrapper {
    notify: Arc<Notify>,
}

impl Drop for StopSensorHandleWrapper {
    fn drop(&mut self) {
        self.notify.notify_waiters();
    }
}

#[cxx::bridge]
mod ffi {
    #[namespace = "ffi"]
    pub struct Sensors {
        pub test_result: Vec<TestResult>,
        pub localization: Vec<LocalizationInfo>,
        pub perception: Vec<PerceptionInfo>,
        pub chassis: Vec<ChassisInfo>,
        pub collision_event: Vec<CollisionEvent>,
        pub lane_invasion_event: Vec<LaneInvasionEvent>,
        pub camera_images: Vec<Image>,
    }

    #[namespace = "ffi"]
    pub struct TestResult {
        pub success: bool,
        pub successful_criteria: Vec<String>,
        pub failed_criteria: Vec<String>,
    }

    #[namespace = "ffi"]
    pub struct LocalizationInfo {
        pub timestamp: u64,
        pub cartesian_lla: [f64; 3],
        pub position_xyz: [f64; 3],
        pub orientation_wxyz: [f64; 4],
        pub linear_vel_xyz: [f64; 3],
        pub linear_accel_xyz: [f64; 3],
        pub angular_vel_xyz: [f64; 3],
    }

    #[namespace = "ffi"]
    pub struct PerceptionInfo {
        pub timestamp: u64,
        pub obstacles: Vec<ObstacleInfo>,
        pub traffic_light: Vec<TrafficLightInfo>,
    }

    #[namespace = "ffi"]
    pub struct ObstacleInfo {
        pub id: String,
        pub o_type: ObstacleType,
        pub is_static: bool,
        pub bounding_box: BoundingBox,
        pub position_xyz: [f64; 3],
        pub orientation_wxyz: [f64; 4],
        pub linear_vel_xyz: [f64; 3],
    }

    #[namespace = "ffi"]
    pub enum ObstacleType {
        Unknow,
        Vehicle,
        Pedestrian,
    }

    #[namespace = "ffi"]
    pub struct BoundingBox {
        pub position_xyz: [f64; 3],
        pub orientation_wxyz: [f64; 4],
        pub extent_xyz: [f64; 3],
    }

    #[namespace = "ffi"]
    pub struct TrafficLightInfo {
        pub timestamp: u64,
        pub position_xyz: [f64; 3],
        pub orientation_wxyz: [f64; 4],
        pub state: TrafficLightState,
    }

    #[namespace = "ffi"]
    pub enum TrafficLightState {
        Red,
        Yellow,
        Green,
        Off,
        Unknown,
    }

    #[namespace = "ffi"]
    pub struct ChassisInfo {
        pub timestamp: u64,
        pub control: VehicleControl,
    }

    #[namespace = "ffi"]
    pub struct VehicleControl {
        pub throttle: f64,
        pub steer: f64,
        pub brake: f64,
        pub hand_brake: bool,
        pub reverse: bool,
        pub manual_gear_shift: bool,
        pub gear: i32,
    }

    #[namespace = "ffi"]
    pub struct CollisionEvent {
        pub timestamp: u64,
        pub normal_impulse: [f64; 3],
    }

    #[namespace = "ffi"]
    pub struct LaneInvasionEvent {
        pub timestamp: u64,
        pub lane_change: Vec<LaneChangeType>,
    }

    #[namespace = "ffi"]
    pub enum LaneChangeType {
        None,
        Right,
        Left,
        Both,
    }

    #[namespace = "ffi"]
    pub struct Image {
        pub timestamp: u64,
        pub camera_id: String,
        pub width: u32,
        pub height: u32,
        pub is_gray: bool,
        pub data: Vec<u8>,
    }

    #[namespace = "ffi"]
    extern "Rust" {
        type ClientWraper;

        fn connect(uri: &str, timeout_ms: u64) -> Result<Box<ClientWraper>>;

        fn recycle(
            vch: Box<VehicleControlHandleWrapper>,
            sh: Box<SensorHandleWrapper>,
        ) -> Box<ClientWraper>;

        fn create_vehicle(
            client: Box<ClientWraper>,
            client_id: &str,
            vehicle_type: &str,
            position_xyz: [f64; 3],
            orientation_wxyz: [f64; 4],
            timeout_ms: u64,
        ) -> Result<Box<HandleWrapper>>;

        fn get_vehicle(
            client: Box<ClientWraper>,
            client_id: &str,
            token: &str,
            timeout_ms: u64,
        ) -> Result<Box<HandleWrapper>>;

        type HandleWrapper;

        fn get_vehicle_control_handle(
            self: &mut HandleWrapper,
        ) -> Result<Box<VehicleControlHandleWrapper>>;

        fn get_sensor_handle(self: &mut HandleWrapper) -> Result<Box<SensorHandleWrapper>>;

        fn get_stop_sensor_handle(self: &mut HandleWrapper)
            -> Result<Box<StopSensorHandleWrapper>>;

        type VehicleControlHandleWrapper;

        fn set_control(
            self: &mut VehicleControlHandleWrapper,
            control: &VehicleControl,
            timeout_ms: u64,
        ) -> Result<()>;

        fn set_transform(
            self: &mut VehicleControlHandleWrapper,
            position_xyz: &[f64; 3],
            orientation_wxyz: &[f64; 4],
        ) -> Result<()>;

        type SensorHandleWrapper;

        fn get_sensors(self: &mut SensorHandleWrapper) -> Result<Sensors>;

        type StopSensorHandleWrapper;
    }
}
